using BepuPhysics.CollisionDetection;
using BepuPhysics.Constraints;
using BepuUtilities;
using BepuUtilities.Collections;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;

namespace BepuPhysics
{
    public unsafe struct ConstraintReference
    {
        internal TypeBatch* typeBatchPointer;
        public ref TypeBatch TypeBatch
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return ref *typeBatchPointer;
            }
        }
        public readonly int IndexInTypeBatch;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ConstraintReference(TypeBatch* typeBatchPointer, int indexInTypeBatch)
        {
            this.typeBatchPointer = typeBatchPointer;
            IndexInTypeBatch = indexInTypeBatch;
        }
    }

    public struct ConstraintLocation
    {
        // 请注意,其中包含了类型id,即使我们可以从类型参数中提取它。
        // 这对于主体存储器交换引起的引用改变是必需的-在每个主体连接中包括类型元数据是低效的,
        // 所以取而代之的是,我们缓存了一个类型id。
        // (您可以将这些内容打包一下-可以说,给定类型的约束不能超过2^24个,约束类型不能超过2^8个……
        // 除非另有证明,否则它没有那么有价值。)
        public int SetIndex;
        public int BatchIndex;
        public int TypeId;
        public int IndexInTypeBatch;
    }

    public partial class Solver
    {

        /// <summary>
        /// 包含所有约束集的缓冲区。第一个时隙专用于活动集;后续时隙可以被与非活动岛相关联的约束所占用。
        /// </summary>
        public Buffer<ConstraintSet> Sets;

        /// <summary>
        /// 获取对存储在第一个集槽中的活动约束集的引用。
        /// </summary>
        public ref ConstraintSet ActiveSet { get { return ref Sets[0]; } }

        // 注意,活动集的引用句柄存储在约束集之外;
        // 非活动孤岛不存储引用的句柄,因为从未添加任何新约束。
        internal QuickList<IndexSet> batchReferencedHandles;

        public TypeProcessor[] TypeProcessors;

        internal Bodies bodies;
        internal PairCache pairCache;
        internal IslandAwakener awakener;

        /// <summary>
        /// 创建新约束时要从中检索约束句柄的池。
        /// </summary>
        public IdPool HandlePool;
        internal BufferPool pool;
        public Buffer<ConstraintLocation> HandleToConstraint;

        /// <summary>
        /// 获取诉诸后备求解器之前允许的最大求解器批次数。
        /// 如果单个实体受多个FallbackBatchThreshold约束,则所有超出FallbackBatchThreshold的约束都将放入回退批处理中。
        /// 回退批处理使用不同的解算器,该解算器可以在单个批处理中处理影响单个实体的多个约束,从而以收敛速度为代价实现更大的并行性。
        /// </summary>
        public int FallbackBatchThreshold { get; private set; }


        int iterationCount;
        /// <summary>
        /// 获取或设置每次调用Update要计算的求解器迭代次数。
        /// </summary>
        public int IterationCount
        {
            get { return iterationCount; }
            set
            {
                if (value < 1)
                {
                    throw new ArgumentException("Iteration count must be positive.");
                }
                iterationCount = value;
            }
        }

        int minimumCapacityPerTypeBatch;
        /// <summary>
        /// 获取或设置在任何新类型批处理中初始分配的最小空间量(以约束为单位)。
        /// </summary>
        public int MinimumCapacityPerTypeBatch
        {
            get { return minimumCapacityPerTypeBatch; }
            set
            {
                minimumCapacityPerTypeBatch = Math.Max(1, value);
            }
        }
        int[] minimumInitialCapacityPerTypeBatch = new int[0];

        /// <summary>
        /// 设置初始分配给给定类型的新类型批次的最小容量。
        /// </summary>
        /// <param name="typeId">要检查其初始容量的约束类型的ID。</param>
        /// <param name="minumInitialCapacityForType">该类型使用的最小容量。</param>
        public void SetMinimumCapacityForType(int typeId, int minimumInitialCapacityForType)
        {
            if (typeId < 0)
                throw new ArgumentException("Type id must be nonnegative.");
            if (MinimumCapacityPerTypeBatch < 0)
                throw new ArgumentException("Capacity must be nonnegative.");
            if (typeId >= minimumInitialCapacityPerTypeBatch.Length)
                Array.Resize(ref minimumInitialCapacityPerTypeBatch, typeId + 1);
            minimumInitialCapacityPerTypeBatch[typeId] = minimumInitialCapacityForType;
        }

        /// <summary>
        /// 获取给定类型的最小初始容量。
        /// 返回值是MinimumCapacityPerTypeBatch和SetMinimumCapacityForType为给定类型ID设置的值之间的较大者。
        /// </summary>
        /// <param name="typeId">键入ID以检索MINM</param>
        /// <return>较大的MinimumCapacityPerTypeBatch和由SetMinimumCapacityForType设置的给定类型的最小值。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public int GetMinimumCapacityForType(int typeId)
        {
            // 请注意,这里的性能稍微更重要一些,因此使用内联和异常断言。
            Debug.Assert(typeId >= 0, "Type ids must be nonnegative.");
            if (typeId >= minimumInitialCapacityPerTypeBatch.Length)
                return minimumCapacityPerTypeBatch;
            return Math.Max(minimumInitialCapacityPerTypeBatch[typeId], minimumCapacityPerTypeBatch);
        }
        /// <summary>
        /// 将每个类型的所有初始容量重置为零。使所有约束中的最小容量保持不变。
        /// </summary>
        public void ResetPerTypeInitialCapacities()
        {
            Array.Clear(minimumInitialCapacityPerTypeBatch, 0, minimumInitialCapacityPerTypeBatch.Length);
        }

        /// <summary>
        /// 获取所有集、批处理和类型的约束总数。需要枚举
        /// 所有类型的批次;这可能很昂贵。
        /// </summary>
        public int CountConstraints()
        {
            int count = 0;
            for (int setIndex = 0; setIndex < Sets.Length; ++setIndex)
            {
                ref var set = ref Sets[setIndex];
                if (set.Allocated)
                {
                    for (int batchIndex = 0; batchIndex < set.Batches.Count; ++batchIndex)
                    {
                        ref var batch = ref set.Batches[batchIndex];
                        for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex)
                        {
                            count += batch.TypeBatches[typeBatchIndex].ConstraintCount;
                        }
                    }
                }
            }
            return count;
        }


        Action<int> solveWorker;
        Action<int> incrementalContactUpdateWorker;
        public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold,
            int initialCapacity,
            int initialIslandCapacity,
            int minimumCapacityPerTypeBatch)
        {
            this.iterationCount = iterationCount;
            this.minimumCapacityPerTypeBatch = minimumCapacityPerTypeBatch;
            this.bodies = bodies;
            this.pool = pool;
            HandlePool = new IdPool(128, pool);
            ResizeSetsCapacity(initialIslandCapacity + 1, 0);
            FallbackBatchThreshold = fallbackBatchThreshold;
            ActiveSet = new ConstraintSet(pool, fallbackBatchThreshold + 1);
            batchReferencedHandles = new QuickList<IndexSet>(fallbackBatchThreshold + 1, pool);
            ResizeHandleCapacity(initialCapacity);
            solveWorker = SolveWorker;
            incrementalContactUpdateWorker = IncrementalContactUpdateWorker;
        }

        public void Register<TDescription>() where TDescription : unmanaged, IConstraintDescription<TDescription>
        {
            var description = default(TDescription);
            Debug.Assert(description.ConstraintTypeId >= 0, "Constraint type ids should never be negative. They're used for array indexing.");
            if (TypeProcessors == null || description.ConstraintTypeId >= TypeProcessors.Length)
            {
                // 这将导致一些不必要的大小调整,但这几乎无关紧要。它只在注册时发生一次。
                // 这也意味着我们可以将当前类型处理器长度作为约束批的类型容量的准确度量。
                Array.Resize(ref TypeProcessors, description.ConstraintTypeId + 1);
            }
            if (TypeProcessors[description.ConstraintTypeId] == null)
            {
                var processor = (TypeProcessor)Activator.CreateInstance(description.TypeProcessorType);
                TypeProcessors[description.ConstraintTypeId] = processor;
                processor.Initialize(description.ConstraintTypeId);
            }
            else if (TypeProcessors[description.ConstraintTypeId].GetType() != description.TypeProcessorType)
            {
                throw new ArgumentException(
                    $"Type processor {TypeProcessors[description.ConstraintTypeId].GetType().Name} has already been registered for this description's type id " +
                    $"({typeof(TDescription).Name}, {default(TDescription).ConstraintTypeId}). " +
                    $"Cannot register two types with the same type id.");
            }
        }


        /// <summary>
        /// 获取给定约束句柄是否引用求解器中的约束。
        /// </summary>
        /// <param name="constraintHandle">用于检查求解器中是否存在的约束句柄。</param>
        /// <return>如果约束句柄存在于求解器中,则为True,否则为False。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool ConstraintExists(ConstraintHandle constraintHandle)
        {
            // 具有负集索引的约束位置将映射槽标记为未使用。
            return constraintHandle.Value >= 0 && constraintHandle.Value < HandleToConstraint.Length && HandleToConstraint[constraintHandle.Value].SetIndex >= 0;
        }

        /// <summary>
        /// 获取对与句柄关联的约束的直接引用。
        /// 引用是临时的;任何影响被引用类型Batch的约束移除都可能使索引无效。
        /// </summary>
        /// <typeparam name="T">正在引用的类型批次的类型。</typeparam>
        /// <param name="Handle">约束的句柄索引。</param>
        /// <param name="reference">临时直接引用与约束句柄关联的类型Batch和类型Batch中的索引。
        /// 可能因删除约束而无效。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe void GetConstraintReference(ConstraintHandle handle, out ConstraintReference reference)
        {
            AssertConstraintHandleExists(handle);
            ref var constraintLocation = ref HandleToConstraint[handle.Value];
            reference = new ConstraintReference(Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatchPointer(constraintLocation.TypeId), constraintLocation.IndexInTypeBatch);
        }

        [Conditional("DEBUG")]
        private void ValidateBodyReference(int body, int expectedCount, ref ConstraintBatch batch)
        {
            int referencesToBody = 0;
            for (int i = 0; i < batch.TypeBatches.Count; ++i)
            {
                var instancesInTypeBatch = TypeProcessors[batch.TypeBatches[i].TypeId].GetBodyReferenceCount(ref batch.TypeBatches[i], body);
                Debug.Assert(instancesInTypeBatch + referencesToBody <= expectedCount,
                    "Found an instance of a body index that wasn't expected. Possible upstream bug or memory corruption.");
                referencesToBody += instancesInTypeBatch;
            }
            Debug.Assert(referencesToBody == expectedCount);
        }

        [Conditional("DEBUG")]
        internal unsafe void ValidateExistingHandles(bool activeOnly = false)
        {
            var maxBodySet = activeOnly ? 1 : bodies.Sets.Length;
            for (int i = 0; i < maxBodySet; ++i)
            {
                ref var set = ref bodies.Sets[i];
                if (set.Allocated)
                {
                    for (int j = 0; j < set.Count; ++j)
                    {
                        bodies.ValidateExistingHandle(set.IndexToHandle[j]);
                    }
                }
            }
            // 验证活动batchReferencedHandles集合中引用的实体。
            // 请注意,这仅适用于活动集同步批处理;非活动集和后备批处理不显式跟踪引用的句柄。
            for (int batchIndex = 0; batchIndex < Math.Min(ActiveSet.Batches.Count, FallbackBatchThreshold); ++batchIndex)
            {
                ref var handles = ref batchReferencedHandles[batchIndex];
                ref var batch = ref ActiveSet.Batches[batchIndex];
                for (int i = 0; i < bodies.ActiveSet.Count; ++i)
                {
                    var handle = bodies.ActiveSet.IndexToHandle[i];
                    if (handles.Contains(handle.Value))
                        ValidateBodyReference(i, 1, ref batch);
                    else
                        ValidateBodyReference(i, 0, ref batch);
                }
                // 活动集求解器批处理引用句柄中不应存在非活动实体。
                for (int inactiveBodySetIndex = 1; inactiveBodySetIndex < bodies.Sets.Length; ++inactiveBodySetIndex)
                {
                    ref var bodySet = ref bodies.Sets[inactiveBodySetIndex];
                    if (bodies.Sets[inactiveBodySetIndex].Allocated)
                    {
                        for (int i = 0; i < bodySet.Count; ++i)
                        {
                            Debug.Assert(!handles.Contains(bodySet.IndexToHandle[i].Value), "Bodies in an inactive set should not show up in the active solver set's referenced body handles.");
                        }
                    }
                }
            }
            // 现在,对于所有集,验证彼此的约束和主体参照是否一致和完整。
            ReferenceCollector enumerator;
            int maximumBodiesPerConstraint = 0;
            for (int i = 0; i < TypeProcessors.Length; ++i)
            {
                if (TypeProcessors[i] != null && TypeProcessors[i].BodiesPerConstraint > maximumBodiesPerConstraint)
                    maximumBodiesPerConstraint = TypeProcessors[i].BodiesPerConstraint;
            }
            var constraintBodyReferences = stackalloc int[maximumBodiesPerConstraint];
            enumerator.References = constraintBodyReferences;
            var maxConstraintSet = activeOnly ? 1 : Sets.Length;
            for (int setIndex = 0; setIndex < maxConstraintSet; ++setIndex)
            {
                ref var set = ref Sets[setIndex];
                if (set.Allocated)
                {
                    Debug.Assert(bodies.Sets.Length > setIndex);
                    // 我们将从两个方向进行测试：
                    // 对于每个约束,对于该约束中的每个主体,确认参照主体在其约束列表中包含该约束。
                    // 对于每个主体,对于该主体参照的每个约束,确认参照的约束在其主体列表中包括该主体。
                    ref var bodySet = ref bodies.Sets[setIndex];
                    Debug.Assert(bodySet.Allocated, "For any existing constraint set, there must be an aligned existing body set. Constraints don't exist by themselves.");
                    for (int batchIndex = 0; batchIndex < set.Batches.Count; ++batchIndex)
                    {
                        ref var batch = ref set.Batches[batchIndex];
                        for (int typebatchIndex = 0; typebatchIndex < batch.TypeBatches.Count; ++typebatchIndex)
                        {
                            ref var typeBatch = ref batch.TypeBatches[typebatchIndex];
                            var processor = TypeProcessors[typeBatch.TypeId];
                            for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.ConstraintCount; ++indexInTypeBatch)
                            {
                                enumerator.Index = 0;
                                processor.EnumerateConnectedBodyIndices(ref typeBatch, indexInTypeBatch, ref enumerator);

                                for (int i = 0; i < processor.BodiesPerConstraint; ++i)
                                {
                                    // 激活约束是指具有索引的实体。非活动约束使用句柄。
                                    int bodyIndex;
                                    if (setIndex == 0)
                                    {
                                        bodyIndex = constraintBodyReferences[i];
                                    }
                                    else
                                    {
                                        ref var referencedBodyLocation = ref bodies.HandleToLocation[constraintBodyReferences[i]];
                                        Debug.Assert(referencedBodyLocation.SetIndex == setIndex, "Any body involved with a constraint should be in the same set.");
                                        bodyIndex = referencedBodyLocation.Index;
                                    }
                                    Debug.Assert(bodySet.BodyIsConstrainedBy(bodyIndex, typeBatch.IndexToHandle[indexInTypeBatch]),
                                        "If a constraint refers to a body, the body should refer to the constraint.");
                                }
                            }
                        }
                    }
                    // 现在是主体->约束方向。
                    for (int bodyIndex = 0; bodyIndex < bodySet.Count; ++bodyIndex)
                    {
                        ref var constraintList = ref bodySet.Constraints[bodyIndex];
                        for (int constraintIndex = 0; constraintIndex < constraintList.Count; ++constraintIndex)
                        {
                            ref var constraintLocation = ref HandleToConstraint[constraintList[constraintIndex].ConnectingConstraintHandle.Value];
                            Debug.Assert(constraintLocation.SetIndex == setIndex, "Any constraint involved with a body should be in the same set.");
                            ref var batch = ref set.Batches[constraintLocation.BatchIndex];
                            ref var typeBatch = ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId]];
                            var processor = TypeProcessors[typeBatch.TypeId];
                            enumerator.Index = 0;
                            processor.EnumerateConnectedBodyIndices(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator);
                            // 活动约束按索引引用实体;非活动约束使用控制柄。
                            int bodyReference = setIndex == 0 ? bodyIndex : bodySet.IndexToHandle[bodyIndex].Value;
                            Debug.Assert(constraintBodyReferences[constraintList[constraintIndex].BodyIndexInConstraint] == bodyReference,
                                "If a body refers to a constraint, the constraint should refer to the body.");
                        }
                    }
                }
            }
        }

        [Conditional("DEBUG")]
        internal void ValidateConstraintMaps(bool activeOnly = false)
        {
            var setCount = activeOnly ? 1 : Sets.Length;
            for (int setIndex = 0; setIndex < setCount; ++setIndex)
            {
                ref var set = ref Sets[setIndex];
                if (set.Allocated)
                {
                    for (int batchIndex = 0; batchIndex < set.Batches.Count; ++batchIndex)
                    {
                        ref var batch = ref set.Batches[batchIndex];
                        for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex)
                        {
                            ref var typeBatch = ref batch.TypeBatches[typeBatchIndex];
                            for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.ConstraintCount; ++indexInTypeBatch)
                            {
                                var handle = typeBatch.IndexToHandle[indexInTypeBatch];
                                ref var constraintLocation = ref HandleToConstraint[handle.Value];
                                Debug.Assert(constraintLocation.BatchIndex == batchIndex);
                                Debug.Assert(constraintLocation.IndexInTypeBatch == indexInTypeBatch);
                                Debug.Assert(constraintLocation.TypeId == typeBatch.TypeId);
                                Debug.Assert(batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId] == typeBatchIndex);
                            }
                        }
                    }
                }
            }
        }

        [Conditional("DEBUG")]
        internal void AssertConstraintHandleExists(ConstraintHandle handle)
        {
            Debug.Assert(ConstraintExists(handle), "Constraint must exist according to Solver.ConstraintExists test.");
            Debug.Assert(handle.Value >= 0 && handle.Value < HandleToConstraint.Length, "Handle must be contained within the handle mapping.");
            ref var location = ref HandleToConstraint[handle.Value];
            Debug.Assert(location.SetIndex >= 0 && location.SetIndex < Sets.Length, "Set index must be within the sets buffer.");
            ref var set = ref Sets[location.SetIndex];
            Debug.Assert(location.BatchIndex >= 0 && location.BatchIndex < set.Batches.Count, "Batch index must be within the set's batches buffer.");
            ref var batch = ref set.Batches[location.BatchIndex];
            Debug.Assert(location.TypeId >= 0 && location.TypeId < batch.TypeIndexToTypeBatchIndex.Length, "Type id must exist within the batch's type id mapping.");
            var typeBatchIndex = batch.TypeIndexToTypeBatchIndex[location.TypeId];
            Debug.Assert(typeBatchIndex >= 0 && typeBatchIndex < batch.TypeBatches.Count, "Type batch index must be a valid index in the type batches list.");
            ref var typeBatch = ref batch.TypeBatches[typeBatchIndex];
            Debug.Assert(typeBatch.IndexToHandle[location.IndexInTypeBatch].Value == handle.Value, "Index->handle mapping in type batch must agree with handle->index mapping.");
        }

        /// <summary>
        /// 尝试为新约束定位一个点。不为约束执行分配。如果不存在批处理,则返回刚好超出现有批处理列表末尾的索引。
        /// </summary>
        /// <return>约束将适合的批处理的索引。</return>
        /// <remarks>This is used by the narrowphase's multithreaded constraint adders to locate a spot for a new constraint without requiring a lock. Only after a candidate is located
        /// 这些系统是否尝试实际声明,从而限制锁定持续时间并增加潜在并行度。</MARKS>
        internal unsafe int FindCandidateBatch(Span<BodyHandle> bodyHandles)
        {
            ref var set = ref ActiveSet;
            GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists);
            var bodyHandlesAsIntegers = MemoryMarshal.Cast<BodyHandle, int>(bodyHandles);
            for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex)
            {
                if (batchReferencedHandles[batchIndex].CanFit(bodyHandlesAsIntegers))
                    return batchIndex;
            }
            // 没有工作的同步批处理。要么有备用批次,要么还没有足够的批次来保证备用批次,并且现有的批次都不适合句柄。
            return synchronizedBatchCount;
        }

        internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle constraintHandle, Span<BodyHandle> bodyHandles, int typeId, out ConstraintReference reference)
        {
            ref var batch = ref ActiveSet.Batches[targetBatchIndex];
            batch.Allocate(constraintHandle, bodyHandles, bodies, typeId, TypeProcessors[typeId], GetMinimumCapacityForType(typeId), pool, out reference);
            if (targetBatchIndex < FallbackBatchThreshold)
            {
                ref var handlesSet = ref batchReferencedHandles[targetBatchIndex];
                for (int j = 0; j < bodyHandles.Length; ++j)
                {
                    handlesSet.Add(bodyHandles[j].Value, pool);
                }
            }
            else
            {
                Debug.Assert(targetBatchIndex == FallbackBatchThreshold);
                ActiveSet.Fallback.AllocateForActive(constraintHandle, bodyHandles, bodies, typeId, pool);
            }
        }

        internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span<BodyHandle> bodyHandles, out ConstraintHandle constraintHandle, out ConstraintReference reference)
        {
            ref var set = ref ActiveSet;
            Debug.Assert(targetBatchIndex <= set.Batches.Count,
                "It should be impossible for a target batch to be generated which is more than one slot beyond the end of the batch list. Possible misuse of FindCandidateBatch.");
            if (targetBatchIndex == set.Batches.Count)
            {
                // 没有可用的批次。必须创建一个新的。
                if (set.Batches.Count == set.Batches.Span.Length)
                    set.Batches.Resize(set.Batches.Count + 1, pool);
                set.Batches.AllocateUnsafely() = new ConstraintBatch(pool, TypeProcessors.Length);
                if (targetBatchIndex < FallbackBatchThreshold)
                {
                    // 此批不是备用批,因此请为其创建索引集。
                    if (set.Batches.Count == batchReferencedHandles.Span.Length)
                        batchReferencedHandles.Resize(set.Batches.Count + 1, pool);
                    batchReferencedHandles.AllocateUnsafely() = new IndexSet(pool, bodies.ActiveSet.Count);
                }
                // 请注意,如果给定索引没有约束批处理,则无法阻止约束添加。这是成功的保证。
            }
            else
            {
                // 只有非回退批处理才能挡路传入限制。无法阻止添加到备用批处理;它允许与同一正文关联的多个约束。
                if (targetBatchIndex < FallbackBatchThreshold)
                {
                    // 此处已存在非后备约束批处理。这可能会失败。
                    if (!batchReferencedHandles[targetBatchIndex].CanFit(MemoryMarshal.Cast<BodyHandle, int>(bodyHandles)))
                    {
                        // 此批无法容纳约束。
                        constraintHandle = new ConstraintHandle(-1);
                        reference = default;
                        return false;
                    }
                }
            }
            constraintHandle = new ConstraintHandle(HandlePool.Take());
            AllocateInBatch(targetBatchIndex, constraintHandle, bodyHandles, typeId, out reference);

            if (constraintHandle.Value >= HandleToConstraint.Length)
            {
                pool.ResizeToAtLeast(ref HandleToConstraint, HandleToConstraint.Length * 2, HandleToConstraint.Length);
                Debug.Assert(constraintHandle.Value < HandleToConstraint.Length, "Handle indices should never jump by more than 1 slot, so doubling should always be sufficient.");
            }
            ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value];
            // 请注意,新约束始终处于活动状态。此时假定所有连接的实体都已强制处于活动状态。
            for (int i = 0; i < bodyHandles.Length; ++i)
            {
                ref var bodyLocation = ref bodies.HandleToLocation[bodyHandles[i].Value];
                Debug.Assert(bodyLocation.SetIndex == 0, "New constraints should only be created involving already-activated bodies.");
            }
            constraintLocation.SetIndex = 0;
            constraintLocation.IndexInTypeBatch = reference.IndexInTypeBatch;
            constraintLocation.TypeId = typeId;
            constraintLocation.BatchIndex = targetBatchIndex;
            return true;
        }


        /// <summary>
        /// 在不唤醒关联孤岛的情况下将描述应用于约束窗。
        /// </summary>
        /// <typeparam name="TDescription">要应用的描述的类型。</typeparam>
        /// <param name="constraintReference">正在更新的约束的引用。</param>
        /// <param name="description">要应用于插槽的说明。</param>
        [MethodImpl(MethodImplOptions.NoInlining)]
        public void ApplyDescriptionWithoutWaking<TDescription>(ref ConstraintReference constraintReference, ref TDescription description)
            where TDescription : unmanaged, IConstraintDescription<TDescription>
        {
            BundleIndexing.GetBundleIndices(constraintReference.IndexInTypeBatch, out var bundleIndex, out var innerIndex);
            // TODO：请注意,允许传入参数以避免低效的值类型便利重载是非常好的。
            // 我们使用ref的原因是JIT没有识别出此实例调用没有变异实例。
            // 它发出一个localsinit和一个副本。
            // 这里的理想解决方案(不同于原始优化器改进)是允许表达处理数据的函数的某种语言功能
            // 以一种通用的方式,没有间接性,也没有引入语法障碍。
            // (如果您接受语法问题,则可能已经传递了一个结构类型,该类型公开了处理描述的接口实现,但不包含其自身的数据。
            // 那个"Executor"类型的清除开销微不足道,即使使用当前的优化器,内联也应该完全消除这种开销。比较增加的复杂性级别
            // 使用IConstraintDescription,只需携带实现静电功能的要求。(C#的未来版本应该会使这种构造更容易处理。)
            description.ApplyDescription(ref constraintReference.TypeBatch, bundleIndex, innerIndex);
        }

        /// <summary>
        /// 在不唤醒关联孤岛的情况下将描述应用于约束窗。
        /// </summary>
        /// <typeparam name="TDescription">要应用的描述的类型。</typeparam>
        /// <param name="constraintHandle">正在更新的约束的句柄。</param>
        /// <param name="description">要应用于插槽的说明。</param>
        public void ApplyDescriptionWithoutWaking<TDescription>(ConstraintHandle constraintHandle, ref TDescription description)
            where TDescription : unmanaged, IConstraintDescription<TDescription>
        {
            GetConstraintReference(constraintHandle, out var constraintReference);
            ApplyDescriptionWithoutWaking(ref constraintReference, ref description);
        }
        /// <summary>
        /// 在不唤醒关联孤岛的情况下将描述应用于约束窗。
        /// </summary>
        /// <typeparam name="TDescription">要应用的描述的类型。</typeparam>
        /// <param name="constraintHandle">正在更新的约束的句柄。</param>
        /// <param name="description">要应用于插槽的说明。</param>
        public void ApplyDescriptionWithoutWaking<TDescription>(ConstraintHandle constraintHandle, TDescription description)
            where TDescription : unmanaged, IConstraintDescription<TDescription>
        {
            ApplyDescriptionWithoutWaking(constraintHandle, ref description);
        }

        /// <summary>
        /// 将描述应用于约束窗,并在必要时唤醒连接的实体。
        /// </summary>
        /// <typeparam name="TDescription">要应用的描述的类型。</typeparam>
        /// <param name="constraintHandle">正在更新的约束的句柄。</param>
        /// <param name="description">要应用于插槽的说明。</param>
        public void ApplyDescription<TDescription>(ConstraintHandle constraintHandle, ref TDescription description)
            where TDescription : unmanaged, IConstraintDescription<TDescription>
        {
            awakener.AwakenConstraint(constraintHandle);
            ApplyDescriptionWithoutWaking(constraintHandle, ref description);
        }
        /// <summary>
        /// 将描述应用于约束窗,并在必要时唤醒连接的实体。
        /// </summary>
        /// <typeparam name="TDescription">要应用的描述的类型。</typeparam>
        /// <param name="constraintHandle">正在更新的约束的句柄。</param>
        /// <param name="description">要应用于插槽的说明。</param>
        public void ApplyDescription<TDescription>(ConstraintHandle constraintHandle, TDescription description)
            where TDescription : unmanaged, IConstraintDescription<TDescription>
        {
            ApplyDescription(constraintHandle, ref description);
        }

        void Add<TDescription>(Span<BodyHandle> bodyHandles, ref TDescription description, out ConstraintHandle handle)
            where TDescription : unmanaged, IConstraintDescription<TDescription>
        {
            ref var set = ref ActiveSet;
            for (int i = 0; i <= set.Batches.Count; ++i)
            {
                if (TryAllocateInBatch(description.ConstraintTypeId, i, bodyHandles, out handle, out var reference))
                {
                    ApplyDescriptionWithoutWaking(ref reference, ref description);
                    return;
                }
            }
            handle = new ConstraintHandle(-1);
            Debug.Fail("The above allocation loop checks every batch and also one index beyond all existing batches. It should be guaranteed to succeed.");
        }

        /// <summary>
        /// 分配约束槽并设置具有指定描述的约束。
        /// </summary>
        /// <typeparam name="TDescription">要添加的约束描述的类型。</typeparam>
        /// <param name="bodyHandles">约束使用的正文句柄。</param>
        /// <return>已分配的约束句柄。</return>
        public ConstraintHandle Add<TDescription>(Span<BodyHandle> bodyHandles, ref TDescription description)
            where TDescription : unmanaged, IConstraintDescription<TDescription>
        {
            Debug.Assert(description.ConstraintTypeId >= 0 && description.ConstraintTypeId < TypeProcessors.Length &&
                TypeProcessors[description.ConstraintTypeId].GetType() == description.TypeProcessorType,
                "The description's constraint type and type processor don't match what has been registered in the solver. Did you forget to register the constraint type?");
            Debug.Assert(bodyHandles.Length == TypeProcessors[description.ConstraintTypeId].BodiesPerConstraint,
                "The number of bodies supplied to a constraint add must match the expected number of bodies involved in that constraint type. Did you use the wrong Solver.Add overload?");
            // 添加约束假定涉及的主体处于活动状态,因此唤醒所有处于休眠状态的对象。
            for (int i = 0; i < bodyHandles.Length; ++i)
            {
                awakener.AwakenBody(bodyHandles[i]);
            }
            Add(bodyHandles, ref description, out var constraintHandle);
            for (int i = 0; i < bodyHandles.Length; ++i)
            {
                var bodyHandle = bodyHandles[i];
                bodies.ValidateExistingHandle(bodyHandle);
                bodies.AddConstraint(bodies.HandleToLocation[bodyHandle.Value].Index, constraintHandle, i);
            }
            return constraintHandle;
        }

        /// <summary>
        /// 分配约束槽并设置具有指定描述的约束。
        /// </summary>
        /// <typeparam name="TDescription">要添加的约束描述的类型。</typeparam>
        /// <param name="bodyHandles">约束引用的正文句柄。</param>
        /// <return>已分配的约束句柄。</return>
        public ConstraintHandle Add<TDescription>(Span<BodyHandle> bodyHandles, TDescription description)
            where TDescription : unmanaged, IConstraintDescription<TDescription>
        {
            return Add(bodyHandles, ref description);
        }

        /// <summary>
        /// 分配一体约束槽并设置具有指定描述的约束。
        /// </summary>
        /// <typeparam name="TDescription">要添加的约束描述的类型。</typeparam>
        /// <param name="bodyHandle">正文已连接到约束。</param>
        /// <return>已分配的约束句柄。</return>
        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandle, ref TDescription description)
            where TDescription : unmanaged, IOneBodyConstraintDescription<TDescription>
        {
            Span<BodyHandle> bodyHandles = stackalloc BodyHandle[] { bodyHandle };
            return Add(bodyHandles, ref description);
        }

        /// <summary>
        /// 分配一体约束槽并设置具有指定描述的约束。
        /// </summary>
        /// <typeparam name="TDescription">要添加的约束描述的类型。</typeparam>
        /// <param name="bodyHandle">约束的第一个正文。</param>
        /// <return>已分配的约束句柄。</return>
        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandle, TDescription description)
            where TDescription : unmanaged, IOneBodyConstraintDescription<TDescription>
        {
            Span<BodyHandle> bodyHandles = stackalloc BodyHandle[] { bodyHandle };
            return Add(bodyHandles, ref description);
        }

        /// <summary>
        /// 分配两实体约束槽并设置具有指定描述的约束。
        /// </summary>
        /// <typeparam name="TDescription">要添加的约束描述的类型。</typeparam>
        /// <param name="bodyHandleA">约束的第一个正文。</param>
        /// <param name="bodyHandleB">约束的第二个正文。</param>
        /// <return>已分配的约束句柄。</return>
        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, ref TDescription description)
            where TDescription : unmanaged, ITwoBodyConstraintDescription<TDescription>
        {
            Span<BodyHandle> bodyHandles = stackalloc BodyHandle[] { bodyHandleA, bodyHandleB };
            return Add(bodyHandles, ref description);
        }

        /// <summary>
        /// 分配两实体约束槽并设置具有指定描述的约束。
        /// </summary>
        /// <typeparam name="TDescription">要添加的约束描述的类型。</typeparam>
        /// <param name="bodyHandleA">约束的第一个正文。</param>
        /// <param name="bodyHandleB">约束的第二个正文。</param>
        /// <return>已分配的约束句柄。</return>
        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, TDescription description)
            where TDescription : unmanaged, ITwoBodyConstraintDescription<TDescription>
        {
            return Add(bodyHandleA, bodyHandleB, ref description);
        }

        /// <summary>
        /// 分配三体约束槽并设置具有指定描述的约束。
        /// </summary>
        /// <typeparam name="TDescription">要添加的约束描述的类型。</typeparam>
        /// <param name="bodyHandleA">约束的第一个正文。</param>
        /// <param name="bodyHandleB">约束的第二个正文。</param>
        /// <param name="bodyHandleC">约束的第三个正文。</param>
        /// <return>已分配的约束句柄。</return>
        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, ref TDescription description)
            where TDescription : unmanaged, IThreeBodyConstraintDescription<TDescription>
        {
            Span<BodyHandle> bodyHandles = stackalloc BodyHandle[] { bodyHandleA, bodyHandleB, bodyHandleC };
            return Add(bodyHandles, ref description);
        }

        /// <summary>
        /// 分配三体约束槽并设置具有指定描述的约束。
        /// </summary>
        /// <typeparam name="TDescription">要添加的约束描述的类型。</typeparam>
        /// <param name="bodyHandleA">约束的第一个正文。</param>
        /// <param name="bodyHandleB">约束的第二个正文。</param>
        /// <param name="bodyHandleC">约束的第三个正文。</param>
        /// <return>已分配的约束句柄。</return>
        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, TDescription description)
            where TDescription : unmanaged, IThreeBodyConstraintDescription<TDescription>
        {
            return Add(bodyHandleA, bodyHandleB, bodyHandleC, ref description);
        }

        /// <summary>
        /// 分配四体约束槽并设置具有指定描述的约束。
        /// </summary>
        /// <typeparam name="TDescription">要添加的约束描述的类型。</typeparam>
        /// <param name="bodyHandleA">约束的第一个正文。</param>
        /// <param name="bodyHandleB">约束的第二个正文。</param>
        /// <param name="bodyHandleC">约束的第三个正文。</param>
        /// <param name="bodyHandleD">约束的第四个正文。</param>
        /// <return>已分配的约束句柄。</return>
        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, BodyHandle bodyHandleD, ref TDescription description)
            where TDescription : unmanaged, IFourBodyConstraintDescription<TDescription>
        {
            Span<BodyHandle> bodyHandles = stackalloc BodyHandle[] { bodyHandleA, bodyHandleB, bodyHandleC, bodyHandleD };
            return Add(bodyHandles, ref description);
        }

        /// <summary>
        /// 分配四体约束槽并设置具有指定描述的约束。
        /// </summary>
        /// <typeparam name="TDescription">要添加的约束描述的类型。</typeparam>
        /// <param name="bodyHandleA">约束的第一个正文。</param>
        /// <param name="bodyHandleB">约束的第二个正文。</param>
        /// <param name="bodyHandleC">约束的第三个正文。</param>
        /// <param name="bodyHandleD">约束的第四个正文。</param>
        /// <return>已分配的约束句柄。</return>
        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, BodyHandle bodyHandleD, TDescription description)
            where TDescription : unmanaged, IFourBodyConstraintDescription<TDescription>
        {
            return Add(bodyHandleA, bodyHandleB, bodyHandleC, bodyHandleD, ref description);
        }

        // 它被拆分出来供多线程约束移除器使用。
        internal void RemoveBatchIfEmpty(ref ConstraintBatch batch, int batchIndex)
        {
            if (batch.TypeBatches.Count == 0)
            {
                // 批次中不存在更多限制;我们可能可以删除此批次。
                // 仅仅没有约束是不够的。如果某个批次"高于"该批次,我们真的不愿删除该批次：
                // Handle->约束映射涉及批处理索引。如果我们移走这一批,它将每隔一批向下移动一步。
                // 这意味着,为了保持正确性,我们必须更改每个单个批处理中每个单个约束的批处理索引
                // 更高的指数。

                // 这在最坏的情况下是不可行的。

                // 相反,我们只会删除是*最后一个*批次的批次。然后,我们依靠延迟批处理压缩将约束移动到较低级别
                // 随时间流逝的批次。因为它一次只处理有限数量的约束,所以没有相同的帧搭载风险。
                // 因此,即使索引较低的批次被清零,来自较高批次的约束也将过滤向下,从而使最高索引约束可能为空。

                // 这是一件相当安全的事。在指数较高的批次仍然非常满的情况下,低指数批次很难以空告终。
                // 事实上,几乎每个批处理为空的实例都会涉及索引最高的批处理。如果不是,它会很高,不会有
                // 它上面有很多约束。延迟压缩很容易处理。
                // 请注意,使用的是缓存的批处理索引,而不是ref。
                ref var set = ref ActiveSet;
                if (batchIndex == set.Batches.Count - 1)
                {
                    // 请注意,当我们删除一个空批次时,它可能会显示另一个空批次。如果发生这种情况,也要删除显示的批次。
                    while (set.Batches.Count > 0)
                    {
                        var lastBatchIndex = set.Batches.Count - 1;
                        ref var lastBatch = ref set.Batches[lastBatchIndex];
                        if (lastBatch.TypeBatches.Count == 0)
                        {
                            lastBatch.Dispose(pool);
                            // 备用批处理没有引用批处理的句柄。
                            if (lastBatchIndex < FallbackBatchThreshold)
                            {
                                batchReferencedHandles[lastBatchIndex].Dispose(pool);
                                --batchReferencedHandles.Count;
                            }
                            --set.Batches.Count;
                            Debug.Assert(set.Batches.Count == batchReferencedHandles.Count ||
                                (set.Batches.Count == FallbackBatchThreshold + 1 && batchReferencedHandles.Count == FallbackBatchThreshold),
                                "All synchronized batches should have a 1:1 mapping with batchReferencedHandles entries, but the fallback batch doesn't have one.");
                        }
                        else
                        {
                            break;
                        }
                    }
                }
            }
        }

        /// <summary>
        /// 从批处理中移除约束,执行任何必要的批处理清理,但不将约束的句柄返回到池。
        /// </summary>
        /// <param name="constraintHandle">要删除的约束的句柄。</param>
        /// <param name="batchIndex">要从中删除的批次的索引。</param>
        /// <param name="typeId">键入要删除的约束的ID。</param>
        /// <param name="indexInTypeBatch">要在其类型Batch中删除的约束的索引。</param>
        internal unsafe void RemoveFromBatch(ConstraintHandle constraintHandle, int batchIndex, int typeId, int indexInTypeBatch)
        {
            ref var batch = ref ActiveSet.Batches[batchIndex];
            if (batchIndex == FallbackBatchThreshold)
            {
                // 如果这是回退批处理,则不跟踪任何引用的句柄。
                // 请注意,我们必须首先从回退中删除,因为它访问批处理的信息。
                ActiveSet.Fallback.Remove(this, pool, ref batch, constraintHandle, typeId, indexInTypeBatch);
                batch.Remove(typeId, indexInTypeBatch, this);
            }
            else
            {
                batch.RemoveWithHandles(typeId, indexInTypeBatch, batchReferencedHandles.GetPointer(batchIndex), this);
            }
            RemoveBatchIfEmpty(ref batch, batchIndex);
        }

        /// <summary>
        /// 移除与给定句柄关联的约束。请注意,这可能会使任何未完成的直接约束引用无效
        /// 通过重新排序要移除的TypeBatch中的约束。
        /// </summary>
        /// <param name="Handle">要从求解器中删除的约束句柄。</param>
        public void Remove(ConstraintHandle handle)
        {
            AssertConstraintHandleExists(handle);
            ref var constraintLocation = ref HandleToConstraint[handle.Value];
            if (constraintLocation.SetIndex > 0)
            {
                // 要删除约束,该约束必须处于活动状态。
                awakener.AwakenConstraint(handle);
            }
            Debug.Assert(constraintLocation.SetIndex == 0);
            ConstraintGraphRemovalEnumerator enumerator;
            enumerator.bodies = bodies;
            enumerator.constraintHandle = handle;
            EnumerateConnectedBodies(handle, ref enumerator);

            pairCache.RemoveReferenceIfContactConstraint(handle, constraintLocation.TypeId);
            RemoveFromBatch(handle, constraintLocation.BatchIndex, constraintLocation.TypeId, constraintLocation.IndexInTypeBatch);
            // 负集索引将Handle->约束映射中的槽标记为未使用。其他值被认为是未定义的。
            constraintLocation.SetIndex = -1;
            HandlePool.Return(handle.Value, pool);
        }

        public void GetDescription<TConstraintDescription>(ConstraintReference constraintReference, out TConstraintDescription description)
            where TConstraintDescription : unmanaged, IConstraintDescription<TConstraintDescription>
        {
            // 请注意,BuildDescription函数的内联行为对于这里的效率至关重要。
            // 如果编译器可以证明BuildDescription函数从未引用任何实例字段,它将省去(可能代价高昂的)初始化。
            // BuildDescription和ConstraintTypeId成员基本上都是静电。如果C#能在没有开销的情况下更清晰地表达这一点,那就太好了。
            BundleIndexing.GetBundleIndices(constraintReference.IndexInTypeBatch, out var bundleIndex, out var innerIndex);
            Debug.Assert(constraintReference.TypeBatch.TypeId == default(TConstraintDescription).ConstraintTypeId, "Constraint type associated with the TConstraintDescription generic type parameter must match the type of the constraint in the solver.");
            default(TConstraintDescription).BuildDescription(ref constraintReference.TypeBatch, bundleIndex, innerIndex, out description);
        }

        public void GetDescription<TConstraintDescription>(ConstraintHandle handle, out TConstraintDescription description)
            where TConstraintDescription : unmanaged, IConstraintDescription<TConstraintDescription>
        {
            // 请注意,BuildDescription函数的内联行为对于这里的效率至关重要。
            // 如果编译器可以证明BuildDescription函数从未引用任何实例字段,它将省去(可能代价高昂的)初始化。
            // BuildDescription和ConstraintTypeId成员基本上都是静电。如果C#能在没有开销的情况下更清晰地表达这一点,那就太好了。
            ref var location = ref HandleToConstraint[handle.Value];
            Debug.Assert(default(TConstraintDescription).ConstraintTypeId == location.TypeId, "Constraint type associated with the TConstraintDescription generic type parameter must match the type of the constraint in the solver.");
            ref var typeBatch = ref Sets[location.SetIndex].Batches[location.BatchIndex].GetTypeBatch(location.TypeId);
            BundleIndexing.GetBundleIndices(location.IndexInTypeBatch, out var bundleIndex, out var innerIndex);
            default(TConstraintDescription).BuildDescription(ref typeBatch, bundleIndex, innerIndex, out description);
        }


        private bool UpdateConstraintsForBodyMemoryMove(int originalIndex, int newIndex)
        {
            // 请注意,此功能需要扫描约束中的实体。这往往是可以的,因为绝大多数约束不超过2个实体。
            // 虽然可以将正文的索引存储在约束中以避免此扫描,但存储该信息需要在添加时收集该信息。
            // 这无论如何都不是不可能的,但是考虑到此函数将倾向于以延迟的方式调用-我们可以控制有多少缓存优化
            // 我们表演。但是,我们无法控制必须执行多少次添加。这些必须立即执行以确保正确性。
            // 换句话说,除了简化存储要求之外,在这里多做一点工作还可以减少所需的总体工作。
            ref var list = ref bodies.ActiveSet.Constraints[originalIndex];
            bool bodyShouldBePresentInFallback = false;
            for (int i = 0; i < list.Count; ++i)
            {
                ref var constraint = ref list[i];
                ref var constraintLocation = ref HandleToConstraint[constraint.ConnectingConstraintHandle.Value];
                // 这确实需要虚拟调用,但是内存交换不应该是非常频繁的事情。
                // (在10000个活动对象的模拟中,每帧数百个调用可能有些矫枉过正。)
                // (此外,这里有足够数量的缓存未命中间接地址,因此虚拟调用几乎是无关紧要的。)
                TypeProcessors[constraintLocation.TypeId].UpdateForBodyMemoryMove(
                    ref ActiveSet.Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId),
                    constraintLocation.IndexInTypeBatch, constraint.BodyIndexInConstraint, newIndex);
                if (constraintLocation.BatchIndex == FallbackBatchThreshold)
                    bodyShouldBePresentInFallback = true;
            }
            return bodyShouldBePresentInFallback;
        }
        /// <summary>
        /// 更改与实体相关联的所有约束的实体参照,以响应其移动到新槽中。
        /// 与现在位于旧窗口处的躯干相关联的约束(如果有)保持不变。
        /// </summary>
        /// <param name="OriginalBodyIndex">移动的实体曾经驻留的内存索引。</param>
        /// <param name="newBodyLocation">移动的正文现在所在的内存索引。</param>
        internal void UpdateForBodyMemoryMove(int originalBodyIndex, int newBodyLocation)
        {
            if (UpdateConstraintsForBodyMemoryMove(originalBodyIndex, newBodyLocation))
            {
                // 移动的约束之一涉及后备批,因此我们需要更新后备批的正文索引。
                ActiveSet.Fallback.UpdateForBodyMemoryMove(originalBodyIndex, newBodyLocation);
            }
        }

        /// <summary>
        /// 更改与两个实体关联的所有约束的实体参照,以响应它们在内存中交换插槽。
        /// </summary>
        /// <param name="a">第一个交换的正文索引。</param>
        /// <param name="b">第二个交换的正文索引。</param>
        internal void UpdateForBodyMemorySwap(int a, int b)
        {
            var aInFallback = UpdateConstraintsForBodyMemoryMove(a, b);
            var bInFallback = UpdateConstraintsForBodyMemoryMove(b, a);
            if (aInFallback && bInFallback)
            {
                ActiveSet.Fallback.UpdateForBodyMemorySwap(a, b);
            }
            else if (aInFallback)
            {
                ActiveSet.Fallback.UpdateForBodyMemoryMove(a, b);
            }
            else if (bInFallback)
            {
                ActiveSet.Fallback.UpdateForBodyMemoryMove(b, a);
            }
        }

        // TODO：使用非固定时间步长一开始并不理想,但是这些缩放函数比它们需要的更糟糕。
        // 不幸的是,更快的替代方案要复杂得多-累积的脉冲需要与热启动一起缩放,以最大限度地减少内存带宽。
        // 另外,所有这些都不使用多线程。
        // 非活动集更加困难-一种选择是以每个集为基础存储缩放并累计,然后仅在缩放变为活动时才处理缩放。

        /// <summary>
        /// 按给定比例缩放与约束集关联的累积脉冲。
        /// </summary>
        /// <param name="set">设置为比例。</param>
        /// <param name="scale">应用于累积脉冲的比例。</param>
        public void ScaleAccumulatedImpulses(ref ConstraintSet set, float scale)
        {
            for (int batchIndex = 0; batchIndex < ActiveSet.Batches.Count; ++batchIndex)
            {
                ref var batch = ref ActiveSet.Batches[batchIndex];
                for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex)
                {
                    ref var typeBatch = ref batch.TypeBatches[typeBatchIndex];
                    TypeProcessors[typeBatch.TypeId].ScaleAccumulatedImpulses(ref typeBatch, scale);
                }
            }
        }

        /// <summary>
        /// 缩放活动集中所有累积的脉冲。
        /// </summary>
        /// <param name="scale">应用于累积脉冲的比例。</param>
        public void ScaleActiveAccumulatedImpulses(float scale)
        {
            ScaleAccumulatedImpulses(ref ActiveSet, scale);
        }

        /// <summary>
        /// 缩放所有约束集中的所有累积脉冲。
        /// </summary>
        /// <param name="scale">应用于累积脉冲的比例。</param>
        public void ScaleAccumulatedImpulses(float scale)
        {
            for (int i = 0; i < Sets.Length; ++i)
            {
                ref var set = ref Sets[i];
                if (set.Allocated)
                    ScaleAccumulatedImpulses(ref set, scale);
            }
        }

        /// <summary>
        /// 枚举与约束关联的累计脉冲。
        /// </summary>
        /// <param name="constraintHandle">要枚举的约束。</param>
        /// <param name="枚举器">要使用的枚举器。</param>
        public void EnumerateAccumulatedImpulses<TEnumerator>(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach<float>
        {
            ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value];
            ref var typeBatch = ref Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId);
            Debug.Assert(constraintLocation.IndexInTypeBatch >= 0 && constraintLocation.IndexInTypeBatch < typeBatch.ConstraintCount, "Bad constraint location; likely some add/remove bug.");
            TypeProcessors[constraintLocation.TypeId].EnumerateAccumulatedImpulses(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator);
        }

        /// <summary>
        /// 收集给定约束的累积脉冲幅值的平方。
        /// </summary>
        /// <param name="constraintHandle">要查找的累积脉冲的约束。</param>
        /// <return>与给定约束关联的累计脉冲的平方大小。</return>
        public unsafe float GetAccumulatedImpulseMagnitudeSquared(ConstraintHandle constraintHandle)
        {
            ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value];
            ref var typeBatch = ref Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId);
            Debug.Assert(constraintLocation.IndexInTypeBatch >= 0 && constraintLocation.IndexInTypeBatch < typeBatch.ConstraintCount, "Bad constraint location; likely some add/remove bug.");
            var typeProcessor = TypeProcessors[constraintLocation.TypeId];
            var impulses = stackalloc float[typeProcessor.ConstrainedDegreesOfFreedom];
            var floatCollector = new FloatCollector(impulses);
            TypeProcessors[constraintLocation.TypeId].EnumerateAccumulatedImpulses(ref typeBatch, constraintLocation.IndexInTypeBatch, ref floatCollector);
            var sumOfSquares = 0f;
            for (int i = 0; i < typeProcessor.ConstrainedDegreesOfFreedom; ++i)
            {
                var impulse = impulses[i];
                sumOfSquares += impulse * impulse;
            }
            return sumOfSquares;
        }

        /// <summary>
        /// 收集给定约束的累积冲量的大小。
        /// </summary>
        /// <param name="constraintHandle">要查找的累积脉冲的约束。</param>
        /// <return>与给定约束关联的累计脉冲的大小。</return>
        public unsafe float GetAccumulatedImpulseMagnitude(ConstraintHandle constraintHandle)
        {
            return (float)Math.Sqrt(GetAccumulatedImpulseMagnitudeSquared(constraintHandle));
        }

        /// <summary>
        /// 按照实体在约束中的引用顺序枚举与约束关联的实体集。
        /// </summary>
        /// <param name="constraintHandle">要枚举的约束。</param>
        /// <param name="枚举器">要使用的枚举器。</param>
        internal void EnumerateConnectedBodies<TEnumerator>(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach<int>
        {
            ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value];
            ref var typeBatch = ref Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId);
            Debug.Assert(constraintLocation.IndexInTypeBatch >= 0 && constraintLocation.IndexInTypeBatch < typeBatch.ConstraintCount, "Bad constraint location; likely some add/remove bug.");
            TypeProcessors[constraintLocation.TypeId].EnumerateConnectedBodyIndices(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator);
        }

        internal void GetSynchronizedBatchCount(out int synchronizedBatchCount, out bool fallbackExists)
        {
            synchronizedBatchCount = Math.Min(ActiveSet.Batches.Count, FallbackBatchThreshold);
            fallbackExists = ActiveSet.Batches.Count > FallbackBatchThreshold;
            Debug.Assert(ActiveSet.Batches.Count <= FallbackBatchThreshold + 1,
                "There cannot be more than FallbackBatchThreshold + 1 constraint batches because that +1 is the fallback batch which contains all remaining constraints.");
        }

        // 请注意,这些都不会影响约束批估计或类型批估计。我们的假设是,这些都太小了,不需要费心去做。
        // 在最坏的情况下,您可能会看到几千字节。存在这些函数的原因是为了处理潜在的价值数兆字节的约束和主体缓冲区。
        // 也许在这个假设后来失效的地方会发生一些奇怪的事情,但我对此表示怀疑。支持它在API复杂性上是有代价的,所以我们不这样做。

        // TODO：鉴于此处使用Clear涉及的细微差别,可能会有将其设置为仅在内部使用的争论。再说一次,我们以前用过这个推理,结果是
        // 人们不得不修改来源。
        /// <summary>
        /// 从解算器中删除所有对象。这意味着清空模拟约束的快速路径。它不会尝试与其他系统保持同步
        /// 这可能取决于约束的存在,例如每个主体的约束列表。
        /// </summary>
        public void Clear()
        {
            ref var activeSet = ref ActiveSet;
            // 后备批处理没有任何引用批处理的句柄。
            GetSynchronizedBatchCount(out var synchronizedBatchCount, out _);
            for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex)
            {
                batchReferencedHandles[batchIndex].Dispose(pool);
            }
            batchReferencedHandles.Clear();
            ActiveSet.Clear(pool);
            // 所有非活动集都将返回池。
            // 它们的分配总是根据实际岛屿大小创建的。
            for (int i = 1; i < Sets.Length; ++i)
            {
                if (Sets[i].Allocated)
                {
                    Sets[i].Dispose(pool);
                }
            }
            HandlePool.Clear();
        }

        /// <summary>
        /// 调整解算器非TypeBatch数据结构的大小。如果给定容量超过当前分配的容量,则分配将增加。
        /// </summary>
        /// <param name="bodyHandleCapacity">要为其分配空间的正文句柄跨度的大小。适用于批引用句柄集。</param>
        /// <param name="constraintHandleCapacity">要为其分配空间的约束句柄数。适用于句柄->约束映射表。</param>
        public void EnsureSolverCapacities(int bodyHandleCapacity, int constraintHandleCapacity)
        {
            if (HandleToConstraint.Length < constraintHandleCapacity)
            {
                pool.ResizeToAtLeast(ref HandleToConstraint, constraintHandleCapacity, HandlePool.HighestPossiblyClaimedId + 1);
            }
            // 请注意,我们不能收缩到车身手柄容量以下,因为手柄分布可能是任意的。
            var targetBatchReferencedHandleSize = Math.Max(bodies.HandlePool.HighestPossiblyClaimedId + 1, bodyHandleCapacity);
            GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists);
            // 回退批处理没有任何引用的句柄。
            for (int i = 0; i < synchronizedBatchCount; ++i)
            {
                batchReferencedHandles[i].EnsureCapacity(targetBatchReferencedHandleSize, pool);
            }
        }

        void ResizeHandleCapacity(int constraintHandleCapacity)
        {
            pool.ResizeToAtLeast(ref HandleToConstraint, constraintHandleCapacity, HandlePool.HighestPossiblyClaimedId + 1);
            for (int i = HandlePool.HighestPossiblyClaimedId + 1; i < HandleToConstraint.Length; ++i)
            {
                // 负集索引将插槽标记为未使用。
                HandleToConstraint[i].SetIndex = -1;
            }
        }

        /// <summary>
        /// 调整解算器非TypeBatch数据结构的大小。如果分配同时适合所有现有条目和给定容量,则允许缩小分配。
        /// 如果给定容量超过当前分配的容量,则分配将增加。
        /// </summary>
        /// <param name="bodyHandleCapacity">要为其分配空间的正文句柄跨度的大小。适用于批引用句柄集。</param>
        /// <param name="constraintHandleCapacity">要为其分配空间的约束句柄数。适用于句柄->约束映射表。</param>
        public void ResizeSolverCapacities(int bodyHandleCapacity, int constraintHandleCapacity)
        {
            var targetConstraintCount = BufferPool.GetCapacityForCount<ConstraintLocation>(Math.Max(constraintHandleCapacity, HandlePool.HighestPossiblyClaimedId + 1));
            if (HandleToConstraint.Length != targetConstraintCount)
            {
                ResizeHandleCapacity(targetConstraintCount);
            }
            // 请注意,我们不能收缩到车身手柄容量以下,因为手柄分布可能是任意的。
            var targetBatchReferencedHandleSize = Math.Max(bodies.HandlePool.HighestPossiblyClaimedId + 1, bodyHandleCapacity);
            GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists);
            // 回退批处理没有任何引用的句柄。
            for (int i = 0; i < synchronizedBatchCount; ++i)
            {
                batchReferencedHandles[i].Resize(targetBatchReferencedHandleSize, pool);
            }
        }

        internal void ResizeSetsCapacity(int setsCapacity, int potentiallyAllocatedCount)
        {
            Debug.Assert(setsCapacity >= potentiallyAllocatedCount && potentiallyAllocatedCount <= Sets.Length);
            setsCapacity = BufferPool.GetCapacityForCount<ConstraintSet>(setsCapacity);
            if (Sets.Length != setsCapacity)
            {
                var oldCapacity = Sets.Length;
                pool.ResizeToAtLeast(ref Sets, setsCapacity, potentiallyAllocatedCount);
                if (oldCapacity < Sets.Length)
                    Sets.Clear(oldCapacity, Sets.Length - oldCapacity); // 我们依赖于默认初始化未使用的插槽。
            }
        }

        // 请注意,以下操作不会对非活动集强制调整大小。非活动集分配的空间刚好足以覆盖岛中的内容。
        // 由于非活动岛屿从不进行增量添加或删除,因此调整其分配大小从来没有任何意义。
        /// <summary>
        /// 确保所有现有活动类型批次达到或超过当前求解器定义的最小容量。容量小于最小值的类型批次将被放大。
        /// </summary>
        public void EnsureTypeBatchCapacities()
        {
            ref var activeSet = ref ActiveSet;
            for (int i = 0; i < activeSet.Batches.Count; ++i)
            {
                activeSet.Batches[i].EnsureTypeBatchCapacities(this);
            }
        }
        /// <summary>
        /// 将当前求解器定义的最小容量应用于现有类型批处理。容量大于最小值而计数小于最小值的类型批次可能会缩小。
        /// 容量小于最小值的类型批次将被放大。
        /// </summary>
        public void ResizeTypeBatchCapacities()
        {
            ref var activeSet = ref ActiveSet;
            for (int i = 0; i < activeSet.Batches.Count; ++i)
            {
                activeSet.Batches[i].ResizeTypeBatchCapacities(this);
            }
        }

        /// <summary>
        /// 将所有池检索的资源返回到池。
        /// </summary>
        /// <remarks>
        /// 在处理之后,求解器不能‘恢复消重状态’以供重复使用。如果要将批量数据返回到池中,同时使求解器处于可用状态,请考虑改用Clear。
        /// </remarks>
        public void Dispose()
        {
            // 请注意,备用批处理没有引用批处理的句柄。
            GetSynchronizedBatchCount(out var synchronizedBatchCount, out _);
            for (int i = 0; i < synchronizedBatchCount; ++i)
            {
                batchReferencedHandles[i].Dispose(pool);
            }
            batchReferencedHandles.Dispose(pool);
            for (int i = 0; i < Sets.Length; ++i)
            {
                if (Sets[i].Allocated)
                    Sets[i].Dispose(pool);
            }
            pool.Return(ref Sets);
            pool.Return(ref HandleToConstraint);
            HandlePool.Dispose(pool);
        }


    }
}
